import math
import numpy as np
import argparse
import yaml
from pathlib import Path
from enum import Enum
import time
import matplotlib.pyplot as plt
import seaborn as sns
import pandas as pd
from functools import wraps
import logging
import torch.multiprocessing as mp
import datetime
import psutil
import gymnasium as gym

from matplotlib.font_manager import FontProperties  # 导入字体模块

import sys, os, signal
# import sys 
# sys.path.insert(0, sys.path[0]+"/../")
os.environ["KMP_DUPLICATE_LIB_OK"] = "TRUE"  # avoid "OMP: Error #15: Initializing libiomp5md.dll, but found libiomp5md.dll already initialized."
curr_path = os.path.dirname(os.path.abspath(__file__))  # current path
parent_path = os.path.dirname(curr_path)  # parent path 
sys.path.append(parent_path)  # add path to system path

from Configs.config import *

# from tree import *
from utils.tree import *

########### Definition ############
# 帧数
FPS = 30 # 30倍放大
# 
wingspan = 1.363 # m # 翼展
LIM_SIZE = wingspan * FPS * 1.5
LIM_VEL = [15, 45]   # m/s  # 30倍放大
LIM_ANGVEL = [-15, 15]  # deg / s
LIM_ACC = [-1, 1]   # vel & ang

class FormationType(Enum):
    wingLike = 'wingLike'
    horiLike = 'horiLike'
    vertLike = 'vertLike'
    rectLike = 'rectLike'
    circLike = 'circLike'
    
class MergedConfig:
    def __init__(self) -> None:
        pass

def str2bool(v):
    '''transfer str to bool for argparse'''
    if isinstance(v, bool):
        return v
    if v.lower() in ('yes', 'True','true','TRUE', 't', 'y', '1'):
        return True
    elif v.lower() in ('no', 'False','false','FALSE', 'f', 'n', '0'):
        return False
    else:
        raise argparse.ArgumentTypeError('Boolean value expected.')


########### utils: Args ############ 
class InitMsgs:
    """ 初始化信息：载具信息、目标信息、障碍信息
    """
    def __init__(self, ports_cfg, scr_cfg, veh_cfg, tar_cfg, obs_cfg, dcs_cfg):
        self.render_ports_cfg = ports_cfg['render_ports']
        self.server_ports_cfg = ports_cfg['server_ports']
        self.decider_ports_cfg = ports_cfg['decider_ports']

        self.screen_init_msg = scr_cfg
        self.vehicle_init_msg = veh_cfg
        self.target_init_msg = tar_cfg
        self.obstacle_init_msg = obs_cfg
        
        self.decision_cfg = dcs_cfg

def process_yaml_cfg(path_to_configs : str = '/root/code/workspace/src/MVS/Configs/', 
                     filename : str = 'InitConfigs.yaml'):
    ''' load yaml config
    '''
    parser = argparse.ArgumentParser(description="InitConfigs")
    parser.add_argument('--yaml', default=path_to_configs + filename, type=str,
                        help='the path of config file')
    args = parser.parse_args()
    if args.yaml is not None:
        with open(args.yaml) as f:
            load_cfg = yaml.load(f, Loader=yaml.FullLoader)
            ports_init_msg = load_cfg['ports_cfg']
            screen_init_msg = load_cfg['screen_cfg']
            vehicle_init_msg = load_cfg['vehicle_cfg']
            target_init_msg = load_cfg['target_cfg']
            obstacle_init_msg = load_cfg['obstacle_cfg']
            decision_cfg = load_cfg['decision_cfg']
            # load algo config
            formation_cfg = FormationConfig()
            general_cfg = GeneralConfig()
            algo_name = decision_cfg['general_cfg']['algo_name']
            algo_mod = __import__(f"algos.{algo_name}.config", fromlist=['AlgoConfig'])  # dynamic loading of modules
            algo_cfg = algo_mod.AlgoConfig()
            dcs_cfgs = {'formation_cfg': formation_cfg,
                        'general_cfg' : general_cfg, 
                        'algo_cfg' : algo_cfg}
            
            # # merge config
            for cfg_type in dcs_cfgs:
                if load_cfg['decision_cfg'][cfg_type] is not None:
                    for k, v in load_cfg['decision_cfg'][cfg_type].items():
                        setattr(dcs_cfgs[cfg_type], k, v)
            
    return InitMsgs(ports_init_msg, screen_init_msg, vehicle_init_msg,target_init_msg,obstacle_init_msg, dcs_cfgs)

def merge_class_attrs(ob1, ob2):
    ob1.__dict__.update(ob2.__dict__)
    return ob1

def create_dirs(cfg):
    curr_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")  # obtain current time
    task_dir = f"{curr_path}/tasks/{cfg.mode.capitalize()}_{cfg.env_name}_{cfg.algo_name}_{curr_time}"
    setattr(cfg, 'task_dir', task_dir)
    Path(cfg.task_dir).mkdir(parents=True, exist_ok=True)
    model_dir = f"{task_dir}/models"
    setattr(cfg, 'model_dir', model_dir)
    res_dir = f"{task_dir}/results"
    setattr(cfg, 'res_dir', res_dir)
    log_dir = f"{task_dir}/logs"
    setattr(cfg, 'log_dir', log_dir)
    traj_dir = f"{task_dir}/traj"
    setattr(cfg, 'traj_dir', traj_dir)

def get_logger(fpath):
    Path(fpath).mkdir(parents=True, exist_ok=True)
    logger = logging.getLogger(name='r')  # set root logger if not set name
    logger.setLevel(logging.DEBUG)
    formatter = logging.Formatter(
        '%(asctime)s - %(name)s - %(levelname)s: - %(message)s',
        datefmt='%Y-%m-%d %H:%M:%S')
    # output to file by using FileHandler
    fh = logging.FileHandler(f"{fpath}/log.txt")
    fh.setLevel(logging.DEBUG)
    fh.setFormatter(formatter)
    # output to screen by using StreamHandler
    ch = logging.StreamHandler()
    ch.setLevel(logging.DEBUG)
    ch.setFormatter(formatter)
    # add Handler
    logger.addHandler(ch)
    logger.addHandler(fh)
    return logger

# def kill_process(*pids):
#   for pid in pids:
#     a = os.kill(pid, signal.SIGKILL)
#     print('已杀死pid为%s的进程, 返回值是:%s' % (pid, a))

# def get_pid(*ports):
# 	#其中\"为转义"
#     pids = []
#     for port in ports:
#         pid = os.popen("netstat -nlp | grep :%s" % (port)).read().split('/')[-1]
#         if pid:
#             print(port, pid)
#             pids.append(int(pid))
#     return pids

def find_procs_by_port(port):
    """返回占用指定端口的所有进程列表"""
    procs = []
    for proc in psutil.process_iter(['pid', 'name']):
        try:
            for conns in proc.connections(kind='inet'):
                if conns.laddr.port == port:
                    procs.append(proc)
        except psutil.Error:  # 忽略无法访问的进程
            pass
    return procs

def kill_proc_tree(pid, including_parent=True):
    """结束指定PID的进程及其子进程"""
    parent = psutil.Process(pid)
    children = parent.children(recursive=True)
    for child in children:
        child.kill()
    if including_parent:
        parent.kill()

########### utils: DataProcessing ############
def convert_coordinates(cur_pos, targ_cord_orig_pos, rotate_rad_from_orig_to_targ=math.radians( - 90.), is_flip=False):
    """ 坐标转换函数
    """
    rotate_rad = rotate_rad_from_orig_to_targ
    if is_flip:
        # x,y <- x',y'
        rotate_matrix = np.matrix([[math.cos(rotate_rad), - math.sin(rotate_rad)],
                                [math.sin(rotate_rad), math.cos(rotate_rad)]], dtype=float)
        new_pos = np.matmul(rotate_matrix, cur_pos) + targ_cord_orig_pos
    else:
        # x,y -> x',y'
        rotate_matrix = np.matrix([[math.cos(rotate_rad), math.sin(rotate_rad)],
                                [ - math.sin(rotate_rad), math.cos(rotate_rad)]], dtype=float)
        new_pos = np.matmul(rotate_matrix, cur_pos - targ_cord_orig_pos)
    return new_pos

def calc_formation_offset(formation_set : dict, self_id : int, heading_rad : float):
    # coordinator： NED
    x_offset, y_offset = 0., 0.
    if self_id != formation_set.leader_id:
        id_offset = self_id - formation_set.leader_id
        if formation_set.formation_type == FormationType.wingLike.value:
            # print("IntoWinglike")
            row = math.ceil(id_offset / 2.)
            col = math.pow(-1, id_offset % 2) * row
            x_offset = - col * formation_set.formation_dist[0] * math.sin(heading_rad) - row * formation_set.formation_dist[1] * math.cos(heading_rad)
            y_offset =   col * formation_set.formation_dist[0] * math.cos(heading_rad) - row * formation_set.formation_dist[1] * math.sin(heading_rad)
        elif formation_set.formation_type == FormationType.horiLike.value:
            # x_offset = - id_offset * formation_set.formation_dist[0] * math.sin(heading_rad)
            # y_offset =   id_offset * formation_set.formation_dist[0] * math.cos(heading_rad)
            row = math.ceil(id_offset / 2.)
            col = math.pow(-1, id_offset % 2) * row
            x_offset = - col * formation_set.formation_dist[0] * math.sin(heading_rad) 
            y_offset =   col * formation_set.formation_dist[0] * math.cos(heading_rad) 

        elif formation_set.formation_type == FormationType.vertLike.value:
            x_offset = - id_offset * formation_set.formation_dist[1] * math.cos(heading_rad)
            y_offset = - id_offset * formation_set.formation_dist[1] * math.sin(heading_rad)
    else:
        x_offset, y_offset = 0., 0.
    return x_offset, y_offset

def state_format(state_raw : dict, targets_pos : dict):
    state_new = {}
    for key, values in state_raw.items():
        sur_map = {}
        # 归一化处理 pos[-10., 10] vel[-15., 15.]
        # state_new[key] = [a - b for a, b in zip(targets_pos[key], values[0:2])] + values[2:]
        state_new[key] = [(a - b)/(FPS * wingspan) for a, b in zip(targets_pos[key], values[0:2])] + [values[2]- 30.] + [values[3]]
        for idx, val in state_raw.items():
            if idx != key:
                # sur_map[idx] = [a - b for a, b in zip(values[0:2], val[0:2])]
                sur_map[idx] = [(a - b)/(FPS * wingspan) for a, b in zip(values[0:2], val[0:2])]
        # print(sur_map)
        state_new[key].append(sur_map)
    
    return state_new

def count_rad(rad):
    if rad > math.pi:
        rad -= math.pi * 2
        return count_rad(rad)
    elif rad < - math.pi:
        rad += math.pi *2
        return count_rad(rad)
    else:
        return rad
    
def count_deg(deg):
    if deg > 180.: #math.pi:
        deg -= 360. # math.pi * 2
        return count_deg(deg)
    elif deg < -180.: #- math.pi:
        deg += 360. # math.pi *2
        return count_deg(deg)
    else:
        return deg

def limit_round(x, bound:list):
    if x >= bound[1]:
        x -= bound[1]
    if x < bound[0]:
        x += bound[1]
    return x

def generate_collision_map(state, region_num=12, domain_radius=wingspan):
    state_new = state[:4]
    obs_map = state[4]
    # print(obs_map)
    radar_length = 10. * wingspan # deg # 5倍翼展
    obs_info, radar_info, collision_map = {}, [radar_length for _ in range(region_num)], [1. for _ in range(region_num)]
    
    for k, v in obs_map.items():
        temp_rad = count_rad(math.atan2(-v[1], -v[0]))
        obs_info[k] = [np.linalg.norm(v), math.degrees(temp_rad)]    # dist, theta(rad)
        # print(obs_info[k])
        # 计算隶属观测区间
        temp_deg = 0.
        if obs_info[k][0] < radar_length:
            # 障碍物角度转换
            temp_deg = limit_round(obs_info[k][1], [0., 360.])
            
            q, r = divmod(temp_deg, 360. / region_num)
            if r > 15.: 
                q = limit_round(q+1, [0, 12])
            
            obs_param = 0.75
            
            q_deg = 360./region_num * q # 主导观测角度
            div_deg = q_deg - temp_deg  # 主导角度与真实障碍中心角偏差值
            
            # print(obs_param * domain_radius, obs_info[k][0])
            
            if obs_param * domain_radius / obs_info[k][0] < 1:  # 不在碰撞预定区域内 # 3/4 翼展
                print("outside Collision!")
                # print(1)
                spread_deg = math.degrees(math.asin(obs_param * domain_radius / obs_info[k][0]))  # 障碍物切线半夹角
                print("SpreadDeg: ", spread_deg)
                temp = obs_info[k][0] - obs_param * domain_radius
                if temp < radar_info[int(q)]:
                    radar_info[int(q)] = temp  # 主导观测区间
                idx = 1
                while True:
                    flag_minor, flag_major = False, False
                    div_deg_minor = abs(div_deg - 15.) + (idx-1) * 30.
                    if div_deg_minor < spread_deg:
                        radar_info[int(limit_round(q-idx, [0, 12]))] = obs_info[k][0] * math.cos(math.radians(div_deg_minor)) - math.pow(math.pow(obs_param * domain_radius, 2) - math.pow(obs_info[k][0]*math.sin(math.radians(div_deg_minor)), 2), 0.5)
                    else:
                        flag_minor = True
                    div_deg_major = abs(div_deg + 15.) + (idx-1) * 30.
                    if div_deg_major < spread_deg:
                        radar_info[int(limit_round(q+idx, [0, 12]))] = obs_info[k][0] * math.cos(math.radians(div_deg_major)) - math.pow(math.pow(obs_param * domain_radius, 2) - math.pow(obs_info[k][0]*math.sin(math.radians(div_deg_major)), 2), 0.5)
                    else:
                        flag_major = True
                    idx += 1
                    if flag_minor and flag_major:
                        break
            else:   # 在预定碰撞区域内
                print("inside Collision!")
                # for i in range(region_num):
                radar_info[int(q)] = 0.
    
    for i in range(region_num):
        if radar_info[i] <= 1. * domain_radius:  # 2倍翼展
            # collision_map[i] = 1-radar_info[i]/(domain_radius) #domain_radius
            # print(radar_info[i])
            collision_map[i] = radar_info[i]/(1. * domain_radius)
    
    state_new += collision_map
        
    return state_new

# 创建边权列表
def generate_edges_list(swarm_info): # [['id', 'init_pos_x', 'init_pos_y',]...] 只拿前三个
    nodes_num = len(swarm_info)
    # graph = np.full((nodes_num, nodes_num), float('inf'))
    edge_list = []
    node_dict = {}
    for i in range(nodes_num):
        node_dict[swarm_info[i][0]] = Node((swarm_info[i][0], swarm_info[i][1], swarm_info[i][2]))
        
        for j in range(nodes_num):
            if j > i:
                dist_x = swarm_info[i][1] - swarm_info[j][1]
                dist_y = swarm_info[i][2] - swarm_info[j][2]
                dist = np.linalg.norm([dist_x, dist_y])
                edge_list.append((swarm_info[i][0], swarm_info[j][0], dist))
        
    return edge_list, node_dict

# 创建加权连通图
def create_graph(edges_list):
    graph = {}  # Dict
    for edge in edges_list:
        u, v, w = edge
        if u not in graph:
            graph[u] = {}
        if v not in graph:
            graph[v] = {}
        graph[u][v] = w
        graph[v][u] = w
    return graph

# 初始化最小生成树
def init_mst():
    mst_edges = [] # 最小生成树的边
    mst_weight = 0 # 最小生成树的权值之和
    return mst_edges, mst_weight

# 使用普里姆算法求最小生成树
def prim(graph, start = None):
    mst_edges, mst_weight = init_mst() # 初始化最小生成树
    visited = set() # 记录已访问过的顶点
    if start:
        start_node = start
    else:
        start_node = list(graph.keys())[0] # 选第一个为起始点
    visited.add(start_node) # 将起始点标记为已访问

    while len(visited) < len(graph): # 当还有未访问过的顶点时循环
        min_weight = float('inf') # 初始化最小权值为无穷大
        min_edge = None # 初始化最小边为None

        for u in visited: # 遍历已访问过的顶点u
            for v in graph[u]: # 遍历与u相连的顶点v
                if v not in visited: # 如果v还未被访问过
                    if graph[u][v] < min_weight: # 如果(u,v)边的权值比当前最小权值还要小
                        min_weight = graph[u][v] # 更新最小权值为(u,v)边的权值 
                        min_edge = (u,v,min_weight) # 更新最小边为(u,v,min_weight)

        if min_edge: # 如果找到了一条合适的边加入最小生成树中 
            mst_edges.append(min_edge) # 将该边添加到最小生成树的边列表中 
            mst_weight += min_weight  # 将该边的权值累加到最小生成树的总权值中 
            visited.add(min_edge[1])  # 将该边连接到未访问过得那个顶点标记为已访问 

    return mst_edges, mst_weight

# 编队理想队形的MST,用于映射真实节点
def create_formation_baseDST(swarm_info, formation_set):
    swarm_info_array = np.array(swarm_info)
    formation_sp = []
    for idx, number in enumerate(swarm_info_array[:,0]):
        temp_pos = calc_formation_offset(formation_set, number, 0.)
        formation_sp.append([int(idx+1), temp_pos[0], temp_pos[1]])
    f_edges_list, f_nodes_dict = generate_edges_list(formation_sp)
    formation_graph = create_graph(f_edges_list)
    mst_edges, _ = prim(formation_graph, start=formation_set.leader_id)
    # 建立baseDST结构
    baseDST = DST(f_nodes_dict[formation_set.leader_id])
    for node in mst_edges:
        baseDST.add_node(f_nodes_dict[node[1]], f_nodes_dict[node[0]])
    return baseDST, f_nodes_dict, formation_sp


# 更新考虑formation_offset的星形动态边权列表
def update_graph(selfP: Node, mirrP: Node, nodes_dict: dict, visited_dict: dict, graph):  # [真顶点，映射顶点，已访问字典，待修改图]
    selfP_id = selfP.get_value()[0]
    for child in mirrP.get_children():
        if child not in visited_dict.values():
            for viciP_id in graph[selfP_id]:
                if viciP_id not in visited_dict.keys():
                    dist_x = child.get_value()[1] - nodes_dict[viciP_id].get_value()[1]
                    dist_y = child.get_value()[2] - nodes_dict[viciP_id].get_value()[2]
                    dist = np.linalg.norm([dist_x, dist_y])
                    graph[selfP_id][viciP_id] = dist
            return graph, child # 只更新一个子节点
        if child == mirrP.get_children()[-1]:
            return graph, None

# 编队初始化代价最优prim增广树
def formation_prim_tree(graph, nodes_dict: dict, baseDST: DST):  #[顶点分布图，编队设置] 根据编队队形动态分配权值
    # 初始化信息
    mst_edges, mst_weight = init_mst() # 初始化最小生成树
    visited, new_nodes_dict = {}, {} #set() # 记录已访问过的顶点, 完备节点列表
    start_node = list(graph.keys())[0] # 选第一个为起始点
    visited[start_node] = baseDST.get_root() # 将起始点标记为已访问,并建立与baseDST的映射关系    
    new_nodes_dict[start_node] = (nodes_dict[start_node], baseDST.get_root())
    while len(visited) < len(graph): # 当还有未访问过的顶点时循环
        min_weight = float('inf') # 初始化最小权值为无穷大
        min_edge = None # 初始化最小边为None
        tempP = {}
        
        for u, m in visited.items(): # 遍历已访问过的顶点u及其映射baseDST顶点mirror
            if m.is_internal():
                graph, mirrP = update_graph(nodes_dict[u], m, nodes_dict, visited, graph)
                if mirrP:
                    
                    for v in graph[u]: # 遍历与u相连的顶点v
                        if v not in visited.keys(): # 如果v还未被访问过
                            if graph[u][v] < min_weight: # 如果(u,v)边的权值比当前最小权值还要小
                                min_weight = graph[u][v] # 更新最小权值为(u,v)边的权值 
                                min_edge = (u,v,min_weight) # 更新最小边为(u,v,min_weight)
            if min_edge:
                mst_edges.append(min_edge)
                mst_weight += min_weight  # 将该边的权值累加到最小生成树的总权值中 
                tempP[min_edge[1]] = mirrP
                min_edge = None

        if tempP: # 如果找到了一条合适的边加入最小生成树中 
            # mst_edges.append(min_edge) # 将该边添加到最小生成树的边列表中 
            # mst_weight += min_weight  # 将该边的权值累加到最小生成树的总权值中 
            for p, mirr in tempP.items():
                visited[p] = mirr  # 将该边连接到未访问过得那个顶点标记为已访问 
                new_nodes_dict[p] = (nodes_dict[p], mirr)

    # 生成DST结构
    validDST = DST(None)
    for node in mst_edges:
        validDST.add_node(nodes_dict[node[1]], nodes_dict[node[0]])
    
    return validDST, mst_edges, mst_weight, new_nodes_dict

def reward_adaptor(state_new, resize_shrimp, resize_blowup, history) -> float:
    lambda_dis = 0.5
    lambda_con = 0.1
    lambda_vel = 0.1
    
    dis_hist, vel_hist = history
    
    # print(dis_hist)
    r_dis = - lambda_dis * (abs(dis_hist[-1][0]) + abs(dis_hist[-1][1])) / resize_shrimp # np.linalg.norm(state_new[:2])
    if np.linalg.norm(state_new[:2]) < 2. * resize_shrimp * resize_blowup:
        r_con = - lambda_con * abs(np.linalg.norm(dis_hist[-1]) - np.linalg.norm(dis_hist[-2]))
        r_vel = - lambda_vel * abs(vel_hist[-1] - vel_hist[-2])
    else:
        r_con, r_vel = 0., 0.
    reward = r_dis + r_con + r_vel
    # print("r_dis: ",r_dis, "r_con: ",r_con, "r_vel: ",r_vel)
    return reward


if __name__ == '__main__':
    # 读取初始化配置
    init_cfgs = process_yaml_cfg(path_to_configs='/root/code/workspace/src/MVS/Configs/', filename='InitConfigs.yaml')
    
    veh_params = init_cfgs.vehicle_init_msg['params']
    formation_set = init_cfgs.decision_cfg['formation_cfg']
    
    # print(formation_set.formation_type)
    
    start = time.time()
    edges_list, nodes_dict = generate_edges_list(veh_params)
    # print(veh_params[3][:3])
    # print(edges_list)
    # edges_list = [(1,2, 1.3), (1,3, 2.1), (1,4, 0.9), (1,5, 0.7), (1,6, 1.8), (1,7, 2.0), (1,8, 1.5),
    #           (2,3, 0.9), (2,4, 1.8), (2,5, 1.2), (2,6, 2.6), (2,7, 2.3), (2,8, 1.1),
    #           (3,4, 2.6), (3,5, 1.7), (3,6, 2.5), (3,7, 1.9), (3,8, 1.0),
    #           (4,5, 0.7), (4,6, 1.6), (4,7, 1.5), (4,8, 0.9),
    #           (5,6, 0.9), (5,7, 1.1), (5,8, 0.8),
    #           (6,7, 0.6), (6,8, 1.0),
    #           (7,8, 0.5)]
    
    baseDST, f_nodes_dict, formation_sp = create_formation_baseDST(veh_params, formation_set)
    # print(formation_sp)
    # print(baseDST.get_size())
    # print(baseDST.get_root().get_children())
    root = baseDST.get_root()
    # print(baseDST.get_subtree(root))
    
    graph = create_graph(edges_list)
    # print(graph)
    
    validDST, mst_edges, mst_weight, new_nodes_dict = formation_prim_tree(graph, nodes_dict, baseDST)
    # print(validDST.get_subtree(validDST.get_root()))
    
    # print(new_nodes_dict[9][1].get_fromRoot_id())
    
    # ps_ports = ["23846", "23847", "23848", "23849", "23850", "23851"]
    # kill_process(*get_pid(*ps_ports))
    
    # print(list(graph.keys())[0])
    # pre_tree, weight = prim(graph)
    # print(pre_tree, weight / 30)
    # root = nodes_dict[1]
    # tree = DST(root)
    
    # # print(tree._root.get_value())
    
    # for node in pre_tree:
    #     tree.add_node(nodes_dict[node[1]], nodes_dict[node[0]])
    
    # print(time.time() - start)
    # ls = []
    # # print(tree.get_size())
    # baseDST.remove_node(nodes_dict[1])
    # print(baseDST.get_size())
    # print(baseDST.get_root().get_value())
    # print(baseDST.get_subtree(baseDST.get_root()))
    
    # print(len(ls))
    # print(ls[0].get_children()[0].get_value())
    # print(ls[0].get_subtree_size())
    
    state = [0.23265849150853082, -0.19773962310863866, 0.594072903718402, -0.7061895800437435, {1: [-8.699800898182744, -0.3567867199078919], 2: [-5.602085504655296, -0.42666735311733306], 3: [-8.044951868699142, -3.1634729823858136], 4: [-2.568441798165542, -0.484375926970739]}]
    time_start = time.time()
    # print(state[1:6])
    state_new = generate_collision_map(state, 12)  # 一倍翼展
    print("timer:", time.time()-time_start)
    print(state_new)